#!/usr/bin/python

import roslib
import rospy
import xml.dom.minidom

def get_param(name, value=None):
    private = "~%s" % name
    if rospy.has_param(private):
        return rospy.get_param(private)
    elif rospy.has_param(name):
        return rospy.get_param(name)
    else:
        return value

def set_param(name, value):
    private = "~%s" % name
    rospy.set_param(private, value)


def set_param():
    semantic_description = get_param('robot_description_semantic')
    description = get_param('robot_description')
    effector_description = xml.dom.minidom.parseString(semantic_description).getElementsByTagName('end_effector')
    group_description = xml.dom.minidom.parseString(semantic_description).getElementsByTagName('group')
    for group_name in group_description:
        name = group_name.getAttribute('name')
        links = []
        for child in group_name.childNodes:
            if child.nodeName != "chain":
                for child in group_name.childNodes:
                    if child.nodeType is child.TEXT_NODE:
                        continue
                    links.append(child.getAttribute('name'))
            else:
                joints_description = xml.dom.minidom.parseString(description).getElementsByTagName('joint')
                for joint_name in joints_description:
                    if (joint_name.getAttribute('type') != "fixed"):
                        links.append(joint_name.getAttribute('name'))
                    else:
                        pass
        del links[0]
        del links[-1]
        rospy.set_param('~' + name, links)
    for effector_name in effector_description:
        if effector_name.nodeType is effector_name.TEXT_NODE:
            continue
        link_name = effector_name.getAttribute('parent_link')
        group_name = effector_name.getAttribute('parent_group')
        rospy.set_param('/end_effector_link/' + group_name, link_name)



if __name__ == '__main__':
    try:
        rospy.init_node('link_group_publisher')
        set_param()
    except rospy.ROSInterruptException: pass
